/*
  SUMo IMU ROS driver node
  Author: SUMo
  Version: March 16
*/

#define USE_MAHONY
#define USE_COMPASS
#define ZERO_GYRO
#define THRESHOLD 0.0020// For start-up calibration
#define RMS_A -0.99	// RMS filter with Fs = 50 Hz, Tau = 1 sec
#define RMS_B  0.01

#include <unistd.h>	// for the sleep(second) function
#include <stdio.h>	// for fscanf etc.

// ROS related functions: nodehandle, publisher etc.
#include <ros/ros.h>
#include <tf/tf.h>
#include <sensor_msgs/Imu.h>

// Phidget
#include <phidget21.h>

// Sensor fusion algorithm
#ifdef USE_MAHONY
#include "MahonyAHRS.h"
#else
#include "MadgwickAHRS.h"
#endif

// Function prototypes go here:
//callback that will run if the Spatial is attached to the computer
int CCONV AttachHandler(CPhidgetHandle spatial, void *userptr);

//callback that will run if the Spatial is detached from the computer
int CCONV DetachHandler(CPhidgetHandle spatial, void *userptr);

//callback that will run if the Spatial generates an error
int CCONV ErrorHandler(CPhidgetHandle spatial, void *userptr, int ErrorCode, const char *unknown);

//callback that will run when a new datum arrives
int CCONV SpatialDataHandler(CPhidgetSpatialHandle spatial, void *userptr, 
			     CPhidgetSpatial_SpatialEventDataHandle *data, int count);

// Make a class node to encapsulate all data
class PhidgetIMU_Node{

private:
  // Phidget class sensor handler
  CPhidgetSpatialHandle phidget_handle;
  bool isCalibrated;
  
  // ROS node handler
  ros::NodeHandle pn_;  // private nh for parameters
  ros::NodeHandle gn_;  // global nh for pub and srv
  ros::Publisher imu_pub_;
  sensor_msgs::Imu imu_pose_;
  tf::Quaternion init_orientation;

#ifdef USE_MAHONY
  // Mahony AHRS algorithm object
  MahonyAHRS ahrs_;
#else
  // Madgwick AHRS algorithm object
  MadgwickAHRS ahrs_;
#endif

  // Open up the phidget module
  int init_phidget(void)
  {
    //create the spatial object
    CPhidgetSpatial_create(&phidget_handle);

    //Set the handlers to be run when the device is:
    //-> plugged in or opened from software, 
    //-> unplugged or closed from software,
    //-> generates an error.
    CPhidget_set_OnAttach_Handler((CPhidgetHandle)phidget_handle, AttachHandler, NULL);
    CPhidget_set_OnDetach_Handler((CPhidgetHandle)phidget_handle, DetachHandler, NULL);
    CPhidget_set_OnError_Handler((CPhidgetHandle)phidget_handle, ErrorHandler, NULL);

    //Registers a callback that will run according to the set data rate that will return the spatial data changes
    //Requires the handle for the Spatial, the callback handler function that will be called, 
    //and an arbitrary pointer that will be supplied to the callback function (may be NULL)
    CPhidgetSpatial_set_OnSpatialData_Handler(phidget_handle, SpatialDataHandler, this);

    //open the spatial object for device connections
    CPhidget_open((CPhidgetHandle)phidget_handle, -1);

    ROS_INFO("Waiting for spatial to be attached.... \n");

    return (CPhidget_waitForAttachment((CPhidgetHandle)phidget_handle, 10000));
  }

  // Configure ROS related things then
  void init_ROS(void)
  {
    // Configure ROS publisher
    imu_pub_ = gn_.advertise<sensor_msgs::Imu>("imu_data",50);

    pn_.param("IMU_frameID", imu_pose_.header.frame_id, std::string("imu"));

    // Covariance matrices related constants:
    double angular_velocity_stdev_,        angular_velocity_covariance;
    double linear_acceleration_stdev_,     linear_acceleration_covariance; 
    double orientation_stdev_,             orientation_covariance;

    // Get some parameters from the launch file at run time
    pn_.param("linear_acceleration_stdev", linear_acceleration_stdev_, 0.098); 
    pn_.param("orientation_stdev", orientation_stdev_, 0.035); 
    pn_.param("angular_velocity_stdev", angular_velocity_stdev_, 0.012); 
    
    // Set the covariance matrices
    angular_velocity_covariance = angular_velocity_stdev_ * angular_velocity_stdev_;
    orientation_covariance = orientation_stdev_ * orientation_stdev_;
    linear_acceleration_covariance = linear_acceleration_stdev_ * linear_acceleration_stdev_;
    
    imu_pose_.linear_acceleration_covariance[0] = linear_acceleration_covariance;
    imu_pose_.linear_acceleration_covariance[4] = linear_acceleration_covariance;
    imu_pose_.linear_acceleration_covariance[8] = linear_acceleration_covariance;

    imu_pose_.angular_velocity_covariance[0] = angular_velocity_covariance;
    imu_pose_.angular_velocity_covariance[4] = angular_velocity_covariance;
    imu_pose_.angular_velocity_covariance[8] = angular_velocity_covariance;
    
    imu_pose_.orientation_covariance[0] = orientation_covariance;
    imu_pose_.orientation_covariance[4] = orientation_covariance;
    imu_pose_.orientation_covariance[8] = orientation_covariance;
  }

  // Configure the phidget module
  void config_phidget(int sample_period_msec)
  {
    // Open up a file to load the magnetic calibration parameters
    std::string calib_file;
    pn_.param("calib_file", calib_file, std::string("imu_calib.txt"));
    
    FILE* fd = fopen(calib_file.c_str(), "r");
    if (fd != NULL)
      {
	int count;
	double magF, ofs0, ofs1, ofs2, g0, g1, g2, t0, t1, t2, t3, t4, t5;
	count=fscanf(fd,"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
		     &magF, &ofs0, &ofs1, &ofs2, &g0, &g1, &g2,
		     &t0, &t1, &t2, &t3, &t4, &t5);

	if (count != 13)
	  ROS_ERROR("IMU:compass parameter wrong number");
	
	//Set compass parameters
#define SET_COMPASS CPhidgetSpatial_setCompassCorrectionParameters
	SET_COMPASS(phidget_handle,magF, ofs0, ofs1, ofs2, g0, g1, g2,
		    t0, t1, t2, t3, t4, t5);
	fclose(fd);
      }
    else
      {
 	ROS_WARN("IMU:can't load compass param file, reset to default values");
	SET_COMPASS(phidget_handle,0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
		    0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
      }

    //Set the data rate for the spatial events
    //and start streaming
    CPhidgetSpatial_setDataRate(phidget_handle, sample_period_msec);

  }

public:

  PhidgetIMU_Node(ros::NodeHandle ng, ros::NodeHandle np, int sample_period_msec)
    :phidget_handle(0),isCalibrated(false),pn_(np),gn_(ng),
#ifdef USE_MAHONY
     ahrs_(sample_period_msec*0.001, 0.5, 0.0)// convert to milisec
#else
     ahrs_(sample_period_msec*0.001, 0.1)
#endif
  {
    //Three stages initialization:
    // 1. Open the phidget module but does not start it
    // 2. Configure the ros publisher and message
    // 3. Configure the phidget module and start running
    int result;
    const char* err;
    if((result = init_phidget()))
      {
	CPhidget_getErrorDescription(result, &err);
	ROS_ERROR("Problem waiting for attachment: %s\n", err);
      }
    //2.
    init_ROS();
    //3.
    config_phidget(sample_period_msec);
  }

  ~PhidgetIMU_Node()
  {
    CPhidget_close((CPhidgetHandle)phidget_handle);
    CPhidget_delete((CPhidgetHandle)phidget_handle);
  }
  
  //Sensor fusion algorithm, and publish the data
  //Get called in the callback function
  void processIMUdatum(CPhidgetSpatial_SpatialEventDataHandle sd)
  {
    //ROS_INFO("Number of Data Packets in this event: %d\n", count);
    imu_pose_.header.stamp = ros::Time::now();
#define ax  sd->acceleration[0]
#define ay (sd->acceleration[1])
#define az (sd->acceleration[2])

#define mx (sd->magneticField[0])
#define my (sd->magneticField[1])
#define mz (sd->magneticField[2])
#define Gyro imu_pose_.angular_velocity  // Easier to read

    Gyro.x =  sd->angularRate[0] * 0.0174532925; // convert from deg/s to rad/s
    Gyro.y =  sd->angularRate[1] * 0.0174532925;
    Gyro.z =  sd->angularRate[2] * 0.0174532925;

// Mahony's DCM algorithm to compute Orientation in quaternion
#ifdef USE_COMPASS
    if(sd->magneticField[0] < 5000.0) // when the magneticfield reading is valid
      {
	ahrs_.AHRSupdate(Gyro.x, Gyro.y, Gyro.z,
		         ax, ay, az, mx, my, mz);
      }
    else
#endif
      {// then use only gyro and accelerometer
	ahrs_.AHRSupdateIMU(Gyro.x, Gyro.y, Gyro.z, ax, ay, az);
      }
    
    if (!isCalibrated)
      {
	double eW,eX,eY,eZ, error;
	static double old_error =0;
	eW = 1000*(ahrs_.q0 - imu_pose_.orientation.w);
	eX = 1000*(ahrs_.q1 - imu_pose_.orientation.x);
	eY = 1000*(ahrs_.q2 - imu_pose_.orientation.y);
  	eZ = 1000*(ahrs_.q3 - imu_pose_.orientation.z);
	
	error = eW*eW+eX*eX+eY*eY+eZ*eZ;
	error = RMS_B * error - RMS_A * old_error;
	ROS_INFO("RMS error = &=%lf", error);
	if( error < THRESHOLD )
	  {	
	    isCalibrated = true;
	    tf::quaternionMsgToTF(imu_pose_.orientation, init_orientation);
	    init_orientation = init_orientation.inverse();//for convenience
	  }
	old_error = error;
      }

    imu_pose_.linear_acceleration.x = ax;
    imu_pose_.linear_acceleration.y = ay;
    imu_pose_.linear_acceleration.z = az;
    
    imu_pose_.orientation.w = ahrs_.q0;
    imu_pose_.orientation.x = ahrs_.q1;
    imu_pose_.orientation.y = ahrs_.q2;
    imu_pose_.orientation.z = ahrs_.q3;
  
    if (isCalibrated)
      {
	tf::Quaternion current_orientation;
	tf::quaternionMsgToTF(imu_pose_.orientation, current_orientation);
	// Find relative orientation
	current_orientation = init_orientation * current_orientation;
	tf::quaternionTFToMsg(current_orientation, imu_pose_.orientation);
      }
    imu_pub_.publish(imu_pose_);
  }
};// end of PhidgetIMU_node class

//Main program
int main(int argc, char* argv[])
{
  // Initialize ROS
  ros::init(argc, argv, "imu_node");
  ros::NodeHandle pn("~");
  ros::NodeHandle n;

  PhidgetIMU_Node imu(n, pn, 20);//sample period of 20ms

  // must do this to enable the callbacks
  ros::spin();

  ROS_INFO("IMU node exit");
}

//callback that will run if the Spatial is attached to the computer
int CCONV AttachHandler(CPhidgetHandle spatial, void *userptr)
{
  int serialNo;
  CPhidget_getSerialNumber(spatial, &serialNo);
  ROS_INFO("Spatial %10d attached!", serialNo);
  
#ifdef ZERO_GYRO
  //Zero the gyro first
  //sleep(5);
  CPhidgetSpatial_zeroGyro((CPhidgetSpatialHandle) spatial);

  // Wait 4sec for the gyro stablize
  sleep(4);
  ROS_INFO("Gyro zero done, IMU ready to run");
#endif
  return 0;
}
//callback that will run if the Spatial is detached from the computer
int CCONV DetachHandler(CPhidgetHandle spatial, void *userptr)
{
  int serialNo;
  CPhidget_getSerialNumber(spatial, &serialNo);
  ROS_INFO("Phidget Spatial %10d detached!", serialNo);
  
  return 0;
}

//callback that will run if the Spatial generates an error
int CCONV ErrorHandler(CPhidgetHandle spatial, void *userptr, int ErrorCode, const char *unknown)
{
  ROS_ERROR("Error handled. %d - %s", ErrorCode, unknown);
  return 0;
}

//callback that will run when a new datum arrives
int CCONV SpatialDataHandler(CPhidgetSpatialHandle spatial, void *userptr, 
			     CPhidgetSpatial_SpatialEventDataHandle *data, int count)
{
  PhidgetIMU_Node* ptr = ( PhidgetIMU_Node* ) userptr;
  
  ptr->processIMUdatum(*data);
  return 0;
}
